﻿#include "polygon_fill_algo.h"
#include <algorithm>
#include <limits>

void FieldData::Fill(int32_t y, Key key, float x1, float x2)
{
    if (!CheckValid(y))
    {
        return;
    }
    if (x1 == x2)
    {
        return;
    }
    if (x1 > x2)
    {
        std::swap(x1, x2);
    }
    filled_field_[y][key].emplace_back(x1, x2);
}

void FieldData::DebugPrint()
{
    int32_t y = 0;
    for (auto &it : filled_field_)
    {
        std::cout << y++ << ":";
        for (auto &it2 : it)
        {
            std::cout << it2.first;
            for (auto &it3 : it2.second)
                std::cout << "(" << it3.first << "," << it3.second << ")";
            std::cout << "  ";
        }
        std::cout << std::endl;
    }
}

bool PolygonFillSweepLineImpl::GetMinMaxY(const Polygon &polygon, const Data &data, int32_t &y_min, int32_t &y_max) const
{
    for (auto &p : polygon)
    {
        int32_t y = data.ToSlot(p.z());
        y_min = std::min(y_min, y);
        y_max = std::max(y_max, y);
    }
    return y_min < y_max;
}

void PolygonFillSweepLineImpl::InitEdge(const Data &data, int32_t y_min, const Point &p1, const Point &p2, LinesWithY &lines) const
{
    int32_t y1 = data.ToSlot(p1.z());
    int32_t y2 = data.ToSlot(p2.z());
    if (y1 == y2 || y1 < y_min || y2 < y_min)
    {
        return;
    }
    float x_delta = (p2.x() - p1.x()) / (p2.z() - p1.z()) * data.a_; // 1/k
    if (y1 < y2)
    {
        lines[y1 - y_min].emplace_back(p1.x(), x_delta, y2 - 1);
    }
    else
    {
        lines[y2 - y_min].emplace_back(p2.x(), x_delta, y1 - 1);
    }
}
void PolygonFillSweepLineImpl::InitEdges(int32_t y_min, const Polygon &polygon, const Data &data, LinesWithY &lines) const
{
    for (int32_t i = 0; i + 1 < polygon.size(); ++i)
    {
        const Point &p1 = polygon[i];
        const Point &p2 = polygon[i + 1];
        InitEdge(data, y_min, p1, p2, lines);
    }
    InitEdge(data, y_min, polygon.back(), polygon.front(), lines);
}
void PolygonFillSweepLineImpl::InsertAET(const Lines &lines, Lines &aet) const
{
    // 插入排序
    for (auto &l1 : lines)
    {
        bool inserted = false;
        for (auto it = aet.begin(); it != aet.end(); ++it)
        {
            if (l1.x_left < it->x_left)
            {
                aet.insert(it, l1);
                inserted = true;
                break;
            }
        }
        if (!inserted)
        {
            aet.emplace_back(l1);
        }
    }
}
void PolygonFillSweepLineImpl::DoFill(int32_t y, const Lines &aet, Data &data) const
{
    auto cur = aet.begin();
    auto next = cur;
    while (next != aet.end())
    {
        if (++next == aet.end())
        {
            break;
        }

        float x1 = cur->x_left;
        float x2 = next->x_left;
        data.Fill(y, 0, x1, x2);
        cur = ++next;
    }
}
void PolygonFillSweepLineImpl::RemoveAET(float y, Lines &aet) const
{
    // 删除无效的边
    for (auto it = aet.begin(); it != aet.end();)
    {
        if (it->y_max <= y)
        {
            it = aet.erase(it);
        }
        else
        {
            it++;
        }
    }
}
void PolygonFillSweepLineImpl::UpdateAET(Lines &lines) const
{
    // 更新x
    for (auto &line : lines)
    {
        line.x_left += line.x_delta;
    }
    lines.sort([](const Line &l1, const Line &l2) -> bool {
        return l1.x_left < l2.x_left;
    });
}
bool PolygonFillSweepLineImpl::Fill(const Polygon &polygon, Data &data) const
{
    int32_t y_min = 0;
    int32_t y_max = 0;
    if (!GetMinMaxY(polygon, data, y_min, y_max))
    {
        return false;
    }
    int32_t height = y_max - y_min + 1;
    LinesWithY lines(height);
    InitEdges(y_min, polygon, data, lines);
    Lines aet;
    for (int32_t y = 0; y < height; ++y)
    {
        InsertAET(lines[y], aet);
        DoFill(y, aet, data);
        RemoveAET(y, aet);
        UpdateAET(aet);
    }

    data.DebugPrint();

    return true;
}

bool PolygonFillSweepLineImpl::Fill(const Point &center, float r1, float r2, Data &data) const
{
    if (r2 <= 0 || r1 > r2)
    {
        return false;
    }
    int32_t y_center = data.ToSlot(center.z());
    int32_t y_min = data.ToSlot(center.z() - r2);
    int32_t y_max = data.ToSlot(center.z() + r2);
    int32_t y_1 = data.ToSlot(center.z() - r1);
    for (int32_t y = y_min; y < y_center; ++y)
    {
        int32_t h = y_center - y;
        float dx = std::sqrt((r2 + h) * (r2 - h));
        if (r1 > 0 && y >= y_1)
        {
            int32_t h_1 = y_center - y;
            float dx1 = std::sqrt((r1 + h_1) * (r1 - h_1));
            data.Fill(y, 0, center.x() - dx, center.x() - dx1);
            data.Fill(y, 0, center.x() + dx1, center.x() + dx);
            data.Fill(y_center + y_center - y, 0, center.x() - dx, center.x() - dx1);
            data.Fill(y_center + y_center - y, 0, center.x() + dx1, center.x() + dx);
        }
        else
        {
            data.Fill(y, 0, center.x() - dx, center.x() + dx);
            data.Fill(y_center + y_center - y, 0, center.x() - dx, center.x() + dx);
        }
    }
    data.Fill(y_center, 0, center.x() - r2, center.x() - r1);
    data.Fill(y_center, 0, center.x() + r1, center.x() + r2);

    data.DebugPrint();

    return false;
}

bool PolygonQuerySegmentImpl::Query(const Data &data, const Point &point, Point &closet_point) const
{
    closet_point = point;

    float x = point.x();
    int32_t y = data.ToSlot(point.z());

    float closet_distance = std::numeric_limits<float>::max();
    SegmentList result;

    for (int32_t i = y-1; i <= y+1; ++i)
    {
        if (!data.CheckValid(i))
        {
            continue;
        }
        result.clear();
        for (auto &it: data.filled_field_[i])
        {
            MergeSegment(it.second, result);
        }
        float tx = x;
        for (auto &it: result)
        {
            if (IsSegContain(it, Segment(tx, tx)))
            {
                if (tx - it.first < it.second - tx)
                {
                    tx = it.first - 0.1;
                }
                else
                {
                    tx = it.second + 0.1;
                }
                break;
            }
        }
        float dis = (x - tx) * (x - tx) + (y - i) * (y - i);
        if (dis < closet_distance)
        {
            closet_distance = dis;
            closet_point.SetX(tx);
            closet_point.SetZ(data.ToReal(i));
        }
    }
    
    // TODO make closet_point valid

    return true;
}

void PolygonQuerySegmentImpl::MergeSegment(const SegmentList &segment_list, SegmentList &result) const
{
    for (auto &s1: segment_list)
    {
        if (result.empty())
        {
            result.emplace_back(s1);
        }
        else
        {
            for (auto &s2: result)
            {
                if (IsSegContain(s2, s1))
                {
                    continue;
                }
                if (IsSegCross(s2, s1))
                {// merge
                    s2.first = std::min(s2.first, s1.first);
                    s2.second = std::max(s2.second, s1.second);
                }
                else
                {// new
                    result.emplace_back(s1);
                }
            }
        }
    }
}

bool PolygonQuerySegmentImpl::IsSegCross(const Segment &s1, const Segment &s2) const
{
    return !(s1.second < s2.first || s2.second < s1.first);
}

bool PolygonQuerySegmentImpl::IsSegContain(const Segment &s1, const Segment &s2) const
{
    return (s1.first < s2.first && s2.second < s1.second);
}